/*
 * Copyright 2013 Insu Song. All rights reserved.
 * inssong@gmail.com  http://www.kopo.com  
 * 
 * Redistribution and use in source and binary forms, with or without modification, are
 * permitted provided that the following conditions are met:
 * 
 *    1. Redistributions of source code must retain the above copyright notice, this list of
 *       conditions and the following disclaimer.
 * 
 *    2. Redistributions in binary form must reproduce the above copyright notice, this list
 *       of conditions and the following disclaimer in the documentation and/or other materials
 *       provided with the distribution.
 * 
 * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL ARSHAN POURSOHI OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * 
 * The views and conclusions contained in the software and documentation are those of the
 * authors and should not be interpreted as representing official policies, either expressed
 * or implied.
 */
package com.kopo.droidioiolib;


import java.util.ArrayList;
import java.util.List;

import android.util.Log;

import ioio.lib.api.AnalogInput;
import ioio.lib.api.DigitalInput;
import ioio.lib.api.DigitalOutput;
import ioio.lib.api.DigitalOutput.Spec.Mode;
import ioio.lib.api.PwmOutput;
import ioio.lib.api.exception.ConnectionLostException;
import ioio.lib.util.BaseIOIOLooper;

// A handler implementing interaction with a single IOIO over a single connection period. 
// The interface utilizes a basic workflow for working with a IOIO instance: 
//   as soon as a connection is established, setup(IOIO) will be called. 
//   Then, the loop() method will be called repeatedly as long as the connection is alive. 
//   Last, the disconnected() method will be called upon losing the connection 
//   (as result of physical disconnection, closing the application, etc). 
//   In case a IOIO with an incompatible firmware is encountered, incompatible() will be called instead of setup(IOIO), 
//   and the IOIO instance is entirely useless, until eventually disconnected() gets called.

public class ControlInterface extends BaseIOIOLooper {
	public static ControlInterface me = null;

	public boolean isConnected = true;

	public List<PwmOutput> pwmOutputs = new ArrayList<PwmOutput>();
	public List<DigitalOutput> pwmDirOutputs = new ArrayList<DigitalOutput>();
	public List<PwmOutput> servoOutputs = new ArrayList<PwmOutput>();

	public List<DigitalInput> digitalInputs = new ArrayList<DigitalInput>();
	public List<AnalogInput> analogInputs = new ArrayList<AnalogInput>();
	
    public DigitalOutput led;

    @Override
    public void setup() {
    	System.out.println("IOIOSingle.setup()");

    	// Setup IOIO Pins. Initialize PWM and servo pins
    	isConnected = true;
    	
		try {
	    	//----- ADD PWMs
	    	//pwm1 = ioio_.openPwmOutput(10, 100);   //  in the normal (i.e. not open-drain) mode, 100Hz = 10ms
	    	pwmOutputs.add( ioio_.openPwmOutput(new DigitalOutput.Spec(10, Mode.NORMAL), 100) );   //  in the normal (i.e. not open-drain) mode, 100Hz = 10ms
	    	pwmDirOutputs.add( ioio_.openDigitalOutput(11) );

	    	//pwm2 = ioio_.openPwmOutput(12, 100);   // 10ms, range 0-255
	    	pwmOutputs.add( ioio_.openPwmOutput(new DigitalOutput.Spec(12, Mode.NORMAL), 100));   //  in the normal (i.e. not open-drain) mode, 100Hz = 10ms
	    	pwmDirOutputs.add( ioio_.openDigitalOutput(13) );

	    	//----- ADD Servos
	    	servoOutputs.add( ioio_.openPwmOutput(new DigitalOutput.Spec(6, Mode.NORMAL), 50) );	// 50 Hz = 20ms
	    	servoOutputs.add( ioio_.openPwmOutput(new DigitalOutput.Spec(7, Mode.NORMAL), 50) );	// 50 Hz = 20ms

	    	//--- ADD analog inputs
	    	analogInputs.add(ioio_.openAnalogInput(41));
	    	analogInputs.add(ioio_.openAnalogInput(42));

	    	//--- ADD digital inputs
	    	led = ioio_.openDigitalOutput(0, false);

	    	pwmOutputs.get(0).setDutyCycle(0);
	    	pwmDirOutputs.get(0).write(false);
	    	pwmOutputs.get(1).setDutyCycle(0);
	    	pwmDirOutputs.get(1).write(false);
	    	servoOutputs.get(0).setDutyCycle(0.075f); // 1.5ms for neutral. Range 1ms-2ms
	    	servoOutputs.get(1).setDutyCycle(0.075f); // 1.5ms for neutral. Range 1ms-2ms
		} catch (ConnectionLostException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}    	
    }

    @Override
    public void loop() throws ConnectionLostException {
    	//System.out.println("IOIOSingle.loop()1");

    	if( ControlDevice.me != null )
    	{
	    	if( ControlDevice.currentBehavior != null)
	    	{
	    		//System.out.println("IOIOSingle.loop()2");
	    		if( ControlDevice.currentBehavior.isEnbled() )
	    		{
		    		//System.out.println("IOIOSingle.loop()3");
	    			ControlDevice.currentBehavior.loop();
	    		}
	    	}
    	}

    	/*
    	if( ledState )
    	{
    		ledState = false;
    	}
    	else
    	{
    		ledState = true;
    	}
		setLed(ledState);
    	*/

		try {
			Thread.sleep(200);
		} catch (InterruptedException e) {
		}

    }

    @Override
    public void incompatible() {
    	System.out.println("IOIOSingle.incompatible()");
    	//isConnected = false;
    }

    @Override
    public void disconnected() {
    	System.out.println("IOIOSingle.disconnected()");
    	//isConnected = false;
    }

    public void setPwm(int pwmNo, int dutyCycle, int dir)
    {
    	if( dir == 0)
    		setPwm(pwmNo, dutyCycle, false);
    	else
    		setPwm(pwmNo, dutyCycle, true);
    }
    
    // dutyCycle in percent 0-100
    public void setPwm(int pwmNo, int dutyCycle, boolean dir)
    {
    	if( !isConnected ) return;
    	if( pwmNo >= pwmOutputs.size() ) return;

		try {
			pwmOutputs.get(pwmNo).setDutyCycle( ((float)dutyCycle)/100.0f);
			pwmDirOutputs.get(pwmNo).write(dir);

			//Log.d("kopo.log", "ControlInterface.setPwm: dir=" + dir);
		} catch (ConnectionLostException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
    }

    // Angle 0-180
    public void setServo(int servoNo, int angle)
    {
    	// 0 = 1ms = 1000, 180 = 2ms = 2000
    	// 
    	if( !isConnected ) return;
    	if( servoNo >= servoOutputs.size() ) return;

		try {
			servoOutputs.get(servoNo).setPulseWidth(1000 + (angle*1000)/180);
			//Log.d("kopo.log", "ControlInterface.setServo");
		} catch (ConnectionLostException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
    }

    public boolean readDigitalPin(int inNo)
    {
    	if( !isConnected ) return false;
    	if( inNo >= digitalInputs.size() ) return false;

    	try {
			return this.digitalInputs.get(inNo).read();
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return false;
		} catch (ConnectionLostException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return false;
		}
    }

    public float readAnalogPin(int inNo)
    {
    	if( !isConnected ) return -1;
    	if( inNo >= analogInputs.size() ) return -1;

    	try {
			return this.analogInputs.get(inNo).read();
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return -1;
		} catch (ConnectionLostException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
			return -1;
		}
    }

    // Pin0. Use this for basic connection testing
    public void setLed(boolean onoff)
    {
    	if( !isConnected ) return;

    	try {
			led.write(onoff);
		} catch (ConnectionLostException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
    }

}